% Group 01
%***********************Code 8 ******************************************

% Main File of Hammerstein Spline Adaptive Filter (HSAF)
% Implements a convergence test of a Hammerstein spline adaptive filter (HSAF).
% SAF-NLMS


clc
clear
close all
disp('Hammerstein Spline Adaptive Filter (HSAF)');

%% Parameters setting

% Input parameters -------------------------------------------------------
Lx = 3e5;   % Length of input signal
nRun = 1;    % Number of runs
SNR = 60 ; % SNR

Interpolation_rate = 16;
N=Lx;

% Adaptive filter definition ----------------------------------------------
M = 7 ;       % Length of linear filter
mu0 = 0.05 ;   % Learning rate for linear filter
% mQ0 = 0.1 ; % Learning rate for control points
mQ1 = [0.005];
for qi = 1:1:length(mQ1)
    mu0 = mQ1(qi); % Learning rate for linear filter
    mQ0 = mQ1(qi);
if Lx < N      % Batch for evaluating MSE
    B = 100 ;
else
    B = 4000 ;
end
mu0_EqLMS = 0.05;
mQ0_EqLMS = 0.05;

mu0_LMS = 0.05;
mQ0_LMS = 0.05;

mu0_NLMS = 0.05;
mQ0_NLMS = 0.05;

mu0_WLMS = 0.05;
mQ0_WLMS = 0.05;

mu0_qLMS = 0.2;
mQ0_qLMS = 0.2;

% Spline activation function definition and initialization ----------------

afinit = 0 ;   % Init act. func. -1 0 ... (ONLY -1, bip.sig. or  0 =linear ) 
aftype = 4 ;   % Kind act. f. -1 0 1 2 4 5; (4 = CR-spline, 5 = B-spline )
Slope  = 1 ;   % Slope
DeltaX = 0.2 ; % Delta X
x_range = 2 ;  % Range limit

% Creating the nonlinearity----------------------------------------------
af0 = create_activation_function( afinit, aftype, DeltaX, x_range, Slope,M); % Model
af1 = create_activation_function( afinit, aftype, DeltaX, x_range, Slope,M); % SAF

%% Initialization

% -------Target Definition ----------------------------------------------
TH1 = create_SPL_lms_adaptive_filter_1(M,mu0,mQ0,1e-2,af0); % Target h Model LMS

% TARGET: Nonlinear memoryless function implemented by Spline interpolated LUT
Q0 = [ -2.20
    -2.00
    -1.80
    -1.60
    -1.40
    -1.20
    -1.00
    -0.80
    -0.91
    -0.40
    -0.20
    0.05
    0.0
    -0.40
    0.58
    1.00
    1.00
    1.20
    1.40
    1.60
    1.80
    2.00
    2.20
    ] ;
TH1.af.Q = Q0;
QL = length ( Q0) ; % Number of c o n t r o l points

% ---------SAF definition ----------------------------------------------
delta = 1e-2;
H1 = create_SPL_lms_adaptive_filter_1(M,mu0_EqLMS,mQ0_EqLMS,delta,af1); % HSAF EqLMS
H2 = create_SPL_lms_adaptive_filter_1(M,mu0_EqLMS,mQ0_EqLMS,delta,af1); % HSAF EqLMS

H3 = create_SPL_lms_adaptive_filter_1(M,mu0_LMS,mQ0_LMS,delta,af1); % HSAF LMS
H4 = create_SPL_lms_adaptive_filter_1(M,mu0_LMS,mQ0_LMS,delta,af1); % HSAF LMS

H5 = create_SPL_lms_adaptive_filter_1(M,mu0_NLMS,mQ0_NLMS,delta,af1); % HSAF NLMS
H6 = create_SPL_lms_adaptive_filter_1(M,mu0_NLMS,mQ0_NLMS,delta,af1); % HSAF NLMS

H7 = create_SPL_lms_adaptive_filter_1(M,mu0_WLMS,mQ0_WLMS,delta,af1); % WSAF LMS
H8 = create_SPL_lms_adaptive_filter_1(M,mu0_WLMS,mQ0_WLMS,delta,af1); % WSAF LMS

H9 = create_SPL_lms_adaptive_filter_1(M,mu0_qLMS,mQ0_qLMS,delta,af1); % HSAF qLMS
H10 = create_SPL_lms_adaptive_filter_1(M,mu0_qLMS,mQ0_qLMS,delta,af1); % HSAF qLMS

% Initialize --------------------------------------------------------------
N = Lx + M + 1 ; % Total samples
for i = Lx+1:N
    x(i) =0;
end

dn = zeros(N,1) ;     % Noise desired output array
d = zeros(N,1) ;      % Desired signal array

y_EqLMS = zeros(N,1) ;      % Output array
y1 = zeros(N,1) ;      % Output array
y2 = zeros(N,1) ;      % Output array

y_LMS = zeros(N,1) ;      % Output array
y3 = zeros(N,1) ;      % Output array
y4 = zeros(N,1) ;      % Output array

y_NLMS = zeros(N,1) ;      % Output array
y5 = zeros(N,1) ;      % Output array
y6 = zeros(N,1) ;      % Output array

y_WLMS = zeros(N,1) ;      % Output array
y7 = zeros(N,1) ;      % Output array
y8 = zeros(N,1) ;      % Output array

y_qLMS = zeros(N,1) ;      % Output array
y9 = zeros(N,1) ;      % Output array
y10 = zeros(N,1) ;      % Output array

e_EqLMS = zeros(Lx,1) ;     % Error array
e1 = zeros(Lx,1) ;     % Error array
e2 = zeros(Lx,1) ;     % Error array

e_LMS = zeros(Lx,1) ;     % Error array
e3 = zeros(Lx,1) ;     % Error array
e4 = zeros(Lx,1) ;     % Error array

e_NLMS = zeros(Lx,1) ;     % Error array
e5 = zeros(Lx,1) ;     % Error array
e6 = zeros(Lx,1) ;     % Error array

e_WLMS = zeros(Lx,1) ;     % Error array
e7 = zeros(Lx,1) ;     % Error array
e8 = zeros(Lx,1) ;     % Error array

e_qLMS = zeros(Lx,1) ;     % Error array
e9 = zeros(Lx,1) ;     % Error array
e10 = zeros(Lx,1) ;     % Error array

em_EqLMS = zeros(Lx,1) ;    % Mean square error
em_LMS = zeros(Lx,1) ;    % Mean square error
em_NLMS = zeros(Lx,1) ;    % Mean square error
em_WLMS = zeros(Lx,1) ;    % Mean square error
em_qLMS = zeros(Lx,1) ;    % Mean square error

varW = zeros(M,1) ;   % Variance value of w
qm1 = zeros(QL,1) ;    % Mean value Spline coeff
qm2 = zeros(QL,1) ;    % Mean value Spline coeff
qm3 = zeros(QL,1) ;    % Mean value Spline coeff
qm4 = zeros(QL,1) ;    % Mean value Spline coeff
qm5 = zeros(QL,1) ;    % Mean value Spline coeff
qm6 = zeros(QL,1) ;    % Mean value Spline coeff
qm7 = zeros(QL,1) ;    % Mean value Spline coeff
qm8 = zeros(QL,1) ;    % Mean value Spline coeff
qm9 = zeros(QL,1) ;    % Mean value Spline coeff
qm10 = zeros(QL,1) ;    % Mean value Spline coeff

varQ = zeros(QL,1) ;  % Variance value Spline coeff

%% Main loop --------------------------------------------------------------
disp (' Algorithm start . . . ') ;
t = clock ;

q1=ones(M,1);
eU1=zeros(M,1);

q2=ones(M,1);
eU2=zeros(M,1);

for n = 0 : nRun-1
    fprintf( ' Test nr . %d/%d\n ' , n+1 , nRun) ;
    data=randi([0,1],N,1);
    [send,qam_gray] = qam16gen(N,data);
    x = send;    
    dn = (0.9 * x) + 0.05 * (x .* conj(x)) ; % 复数期望输出，添加噪声..
    dn = awgn(x, SNR,'measured');

    % SAF I.C. ------------------------------------------------------------
    H1.w (:) = 0 ;H1.w ( 1 ) = 0.1 ;
    H2.w (:) = 0 ;H2.w ( 1 ) = 0.1 ;
    H3.w (:) = 0 ;H3.w ( 1 ) = 0.1 ;
    H4.w (:) = 0 ;H4.w ( 1 ) = 0.1 ;
    H5.w (:) = 0 ;H5.w ( 1 ) = 0.1 ;
    H6.w (:) = 0 ;H6.w ( 1 ) = 0.1 ;
    H7.w (:) = 0 ;H7.w ( 1 ) = 0.1 ;
    H8.w (:) = 0 ;H8.w ( 1 ) = 0.1 ;
    H9.w (:) = 0 ;H9.w ( 1 ) = 0.1 ;
    H10.w (:) = 0 ;H10.w ( 1 ) = 0.1 ;

    % Set Activation Func I.C. --------------------------------------------
    H1.af.Q = af0.Q;
    H2.af.Q = af0.Q;
    H3.af.Q = af0.Q;
    H4.af.Q = af0.Q;
    H5.af.Q = af0.Q;
    H6.af.Q = af0.Q;
    H7.af.Q = af0.Q;
    H8.af.Q = af0.Q;
    H9.af.Q = af0.Q;
    H10.af.Q = af0.Q;

    H7.af = create_activation_function(afinit, aftype, DeltaX, x_range, Slope, M);
    H8.af = create_activation_function(afinit, aftype, DeltaX, x_range, Slope, M);

     % HSAF Evaluation -----------------------------------------------------
    for k = 1 : Lx
        % Computing the desired output ------------------------------------
        %[TH1,d(k),snk] = FW_HSPL_F(TH1,x(k)); % Hammerstein model
        
        % Updating HSAF ----------------------------------------------
        [H1,y1(k),e1(k)] = AF_EqLMS_HSPL_F(H1,real(x(k)),real(d(k)+ dn(k)),q1,eU1) ; % SAF EqLMS ( Eqs .(10) and (11))
        [H2,y2(k),e2(k)] = AF_EqLMS_HSPL_F(H2,imag(x(k)),imag(d(k)+ dn(k)),q2,eU2) ; % SAF EqLMS ( Eqs .(10) and (11))
		
        [H3,y3(k),e3(k)] = AF_LMS_HSPL_F(H3,real(x(k)),real(d(k)+ dn(k))) ; % SAF LMS ( Eqs .(10) and (11))
        [H4,y4(k),e4(k)] = AF_LMS_HSPL_F(H4,imag(x(k)),imag(d(k)+ dn(k))) ; % SAF LMS ( Eqs .(10) and (11))

        [H5,y5(k),e5(k)] = AF_NLMS_HSPL_F3(H5,real(x(k)),real(d(k)+ dn(k))) ; % SAF LMS ( Eqs .(10) and (11))
        [H6,y6(k),e6(k)] = AF_NLMS_HSPL_F3(H6,imag(x(k)),imag(d(k)+ dn(k))) ; % SAF LMS ( Eqs .(10) and (11))

        [H7,y7(k),e7(k)] = AF_LMS_WSPL_F(H7,real(x(k)),real(d(k)+ dn(k))) ; % SAF LMS ( Eqs .(10) and (11))
        [H8,y8(k),e8(k)] = AF_LMS_WSPL_F(H8,imag(x(k)),imag(d(k)+ dn(k))) ; % SAF LMS ( Eqs .(10) and (11))

        [H9,y9(k),e9(k)] = AF_qLMS_HSPL_F(H9,real(x(k)),real(d(k)+ dn(k))) ; % SAF LMS ( Eqs .(10) and (11))
        [H10,y10(k),e10(k)] = AF_qLMS_HSPL_F(H10,imag(x(k)),imag(d(k)+ dn(k))) ; % SAF LMS ( Eqs .(10) and (11))

    end
    e_EqLMS = e1 + 1i.*e2;
	y_EqLMS = y1 + 1i.*y2;
    
    e_LMS = e3 + 1i.*e4;
	y_LMS = y3 + 1i.*y4;

    e_NLMS = e5 + 1i.*e6;
	y_NLMS = y5 + 1i.*y6;

    e_WLMS = e7 + 1i.*e8;
	y_WLMS = y7 + 1i.*y8;

    e_qLMS = e9 + 1i.*e10;
	y_qLMS = y9 + 1i.*y10;

    em_EqLMS = em_EqLMS + (abs(e_EqLMS).^2 ) ; % Squared error
    em_LMS = em_LMS + (abs(e_LMS).^2 ) ; % Squared error
    em_NLMS = em_NLMS + (abs(e_NLMS).^2 ) ; % Squared error
    em_WLMS = em_WLMS + (abs(e_WLMS).^2 ) ; % Squared error
    em_qLMS = em_qLMS + (abs(e_qLMS).^2 ) ; % Squared error

    wm = n+111;
    % SAF run-time mean and variance estimation ---------------------------
    wm = (1/(n+1))*H1.w + (n/(n+1))*wm;
    qm1 = (1/(n+1))*H1.af.Q+(n/(n+1))*qm1;
    qm2 = (1/(n+1))*H2.af.Q+(n/(n+1))*qm2;
    qm3 = (1/(n+1))*H3.af.Q+(n/(n+1))*qm3;
    qm4 = (1/(n+1))*H4.af.Q+(n/(n+1))*qm4;
    qm5 = (1/(n+1))*H5.af.Q+(n/(n+1))*qm5;
    qm6 = (1/(n+1))*H6.af.Q+(n/(n+1))*qm6;

    qm7 = (1/(n+1))*H7.af.Q+(n/(n+1))*qm7;
    qm8 = (1/(n+1))*H8.af.Q+(n/(n+1))*qm8;

    qm9 = (1/(n+1))*H9.af.Q+(n/(n+1))*qm9;
    qm10 = (1/(n+1))*H10.af.Q+(n/(n+1))*qm10;
end

em_EqLMS = em_EqLMS/nRun; % MSE
em_LMS = em_LMS/nRun; % MSE
em_NLMS = em_NLMS/nRun; % MSE
em_WLMS = em_WLMS/nRun; % MSE
em_qLMS = em_qLMS/nRun; % MSE

H1.af.Q = qm1;
H2.af.Q = qm2;
H3.af.Q = qm3;
H4.af.Q = qm4;
H5.af.Q = qm5;
H6.af.Q = qm6;
H7.af.Q = qm7;
H8.af.Q = qm8;
H9.af.Q = qm9;
H10.af.Q = qm10;
%-------------------------------------------------------------------------
% Average MSE evaluation
mse = mean(em_EqLMS(end-B-M-1:end-M-1) ) ; % Average MSE
%-------------------------------------------------------------------------
fprintf('\n') ;

%% Results

% -----------------------------------------------------------------------
% Print table of means and variances
% -----------------------------------------------------------------------
fprintf('\n') ;
fprintf( 'Number of iterations = %d\n ' ,nRun) ;
fprintf( ' Learning rates : muW = %5.3 f muQ = %5.3 f \n ' , mu0, mQ0) ;
fprintf( 'Number of filter weights = %d\n ' , M) ;
fprintf( 'AF type = %d\n ' ,aftype) ;
fprintf( ' DeltaX = %4.2 f \n ' ,DeltaX) ;
fprintf( 'SNR_dB = %4.2 f dB\n ' ,SNR) ;
fprintf( ' Steady-state MSE = %5.7f , equal to %5.3f dB\n ' ,mse,10*log10 (mse)) ;
fprintf('\n ') ;
fprintf('Mean and Variance Tables -----------------------------------\n ' ) ;
for i=1:QL
    fprintf('i=%2d q0 =%5.2 f qm =%9.6f varQ = %10.3e \n ',i,TH1.af.Q( i),qm1( i),varQ(i));
end
fprintf('\n');
fprintf('--------------------------------------------------------------\n');


% MSE dB ----------------------------------------------------------------

box('on');
hold on;
hold('all');
ylim([-SNR-5 10]) ;

%em_EqLMS(1:M) = 1e-1;
edb_EqLMS = 10*log10(em_EqLMS);
emf_EqLMS = zeros(size(edb_EqLMS));

%em_LMS(1:M) = 1e-1;
edb_LMS = 10*log10(em_LMS);
emf_LMS = zeros(size(edb_LMS));

%em_NLMS(1:M) = 1e-1;
edb_NLMS = 10*log10(em_NLMS);
emf_NLMS = zeros(size(edb_NLMS));

%em_WLMS(1:M) = 1e-1;
edb_WLMS = 10*log10(em_WLMS);
emf_WLMS = zeros(size(edb_WLMS));

%em_qLMS(1:M) = 1e-1;
edb_qLMS = 10*log10(em_qLMS);
emf_qLMS = zeros(size(edb_qLMS));

lambda = 0.998;
for n = 2:length(em_EqLMS)
    emf_EqLMS(n) = lambda*emf_EqLMS(n-1) + (1-lambda)*edb_EqLMS(n);
    emf_LMS(n) = lambda*emf_LMS(n-1) + (1-lambda)*edb_LMS(n);
    emf_NLMS(n) = lambda*emf_NLMS(n-1) + (1-lambda)*edb_NLMS(n);
    emf_WLMS(n) = lambda*emf_WLMS(n-1) + (1-lambda)*edb_WLMS(n);
    emf_qLMS(n) = lambda*emf_qLMS(n-1) + (1-lambda)*edb_qLMS(n);

end

noiseLevel(1: length(em_EqLMS)) = -SNR;

hold on; grid on; box on;
set(gca, 'FontSize', 12, 'FontWeight', 'bold');
plot(emf_EqLMS,'LineWidth', 2); 
plot(emf_qLMS,'LineWidth', 2); 
plot(emf_LMS,'LineWidth', 2); 
plot(emf_NLMS,'LineWidth', 2); 
plot(emf_WLMS,'LineWidth', 2); 

% -------------------------------------------------------------------
end
plot(noiseLevel, '--', 'Color', [0 0 1], 'LineWidth', 2);
xlim([0 Lx]);
%title('\bf SAF-EqLMS');
xlabel('信号序列'); ylabel('MSE/dB'); legend('MSE', '底噪');
legend('HSAF-EqLMS','HSAF-qLMS','HSAF-LMS','HSAF-NLMS','WSAF-LMS','noisefloor')
fprintf( 'END Hammerstein Spline Adaptive Filter (HSAF) ---------------------\n ') ;

figure
plot(real(dn));hold on;
xlim([0 500]);
plot(real(y_EqLMS));hold on;
legend('desired','output');

signal_power = mean(abs(dn).^2);
SI_power_EqLMS = mean(abs(e_EqLMS).^2);
cancellation_ratio_EqLMS = 10*log10(signal_power/SI_power_EqLMS);

SI_power_qLMS = mean(abs(e_LMS).^2);
cancellation_ratio_LMS = 10*log10(signal_power/SI_power_qLMS);

   
[P1xx,W] = pwelch(dn,[],[],4096,20);
%e1=e1-mean(e1);

[P2xx,W] = pwelch(e_EqLMS,[],[],4096,20);
[P3xx,W] = pwelch(e_LMS,[],[],4096,20);

figure
fsMHz = 1e7;
hold on; grid on; box on;
set(gca, 'FontSize', 12, 'FontWeight', 'bold');
plot([-2048:2047]*fsMHz/4096,10*log10(fftshift(P1xx))-28);grid on;hold on;
plot([-2048:2047]*fsMHz/4096,10*log10(fftshift(P2xx))-16);hold on;
plot([-2048:2047]*fsMHz/4096,10*log10(fftshift(P3xx))-23);hold on;

%title('基于EqLMS算法的数字域自干扰消除曲线');
ylabel('自干扰信号功率/dBm');xlabel('信号频率/Hz');
legend('数字域自干扰消除前','SAF-EqLMS','SAF-LMS');